% 调试gentrajectory函数 % TODO\LM: 改为TestCase并移至test子目录
% FIXME\LM: 杆臂较大时的精度问题

clc
clear
format long

%% 常量
cst.sec2Rad = pi / 3600 / 180;
cst.deg2Rad = pi / 180;
cst_trajGen = load('earthconstants_WGS84.mat', 'omegaIE', 'RE', 'eSq', 'f');

%% 配置
cfg_trajGen.enableCBVdynamic = false;
cfg_trajGen.enableTrajGen = true;
cfg_trajGen.enableIMU = true;
cfg_trajGen.enableDisplm = true;
cfg_trajGen.enableNavigation = true;
cfg_trajGen.IMU.enableErr = true;
cfg_trajGen.IMU.enableNoise = false;
cfg_trajGen.IMU.ignore2OdSensErr = false;
cfg_trajGen.IMU.quantizeOut = false;
cfg_trajGen.IMU.outFilePath = [pwd '\data\IMUOut.txt'];
if cfg_trajGen.IMU.quantizeOut
    cfg_trajGen.IMU.outPrecision = [];
else
    cfg_trajGen.IMU.outPrecision = '%.16e';
end

%% 输入
in_trajGen.genFuncName = 'gentgi_navtest_L_racetrack';
% in_trajGen.genFuncInArg = {50}; % 静止轨迹/gentgi_static
% in_trajGen.genFuncInArg = {[0 1 0]', zeros(3, 0), [10 100 10]};% 直线段/gentgi_navtest_L_racetrack
in_trajGen.genFuncInArg = {[0 1 0]', [0 0 pi/2]', [10 30 30 10]};% L型轨迹/gentgi_navtest_L_racetrack
% in_trajGen.genFuncInArg = {[0 1 0]', [0 0 pi/2; 0 0 -pi/2]', [10 300 300 300 10]};% Z型行驶/gentgi_navtest_L_racetrack
% in_trajGen.genFuncInArg = {[0 0 -pi/2; 0 0 pi/2], [pi/18; pi/18], [0; 0]};% 连续转动轨迹/gentgi_multirot

%% 参量
par_trajGen.samplePeriod = 0.005;
par_trajGen.g = 9.793632; % 重力加速度，m/s^2
par_trajGen.CBVPeriod = 20;

% 加速度计常值误差
par_trajGen.IMU.acc.PS0B = eye(3);
par_trajGen.IMU.acc.KScal0 = ones(3, 1);
par_trajGen.IMU.acc.dfBiasS = zeros(3, 1);
par_trajGen.IMU.acc.dKScalP = [zeros(3, 1) zeros(3, 1)];
par_trajGen.IMU.acc.dKScalN = [zeros(3, 1) zeros(3, 1)];
par_trajGen.IMU.acc.dPSS0 = zeros(3);
par_trajGen.IMU.acc.dfQuantS = zeros(3, 1);
par_trajGen.IMU.acc.lB = zeros(3);
par_trajGen.IMU.acc.KAniso = zeros(3, 1);
par_trajGen.IMU.acc.CBA = NaN(3, 3, 3);
for i = 1:3
    par_trajGen.IMU.acc.CBA(:, :, i) = eye(3);
end

% par_trajGen.IMU.acc.dfBiasS = [1e-4 -5e-3 8e-5]' * par_trajGen.g; % 转换前单位g，转换后单位m/s^2
% par_trajGen.IMU.acc.KScal0 = [1e4 1e4 1e4]' / par_trajGen.g; % 转换前单位(^/s)/(g)，转换后单位(^/s)/(m/s^2)
% par_trajGen.IMU.acc.dSF = [-2e-4 5e-4 -5e-5]';
% par_trajGen.IMU.acc.MA = [0 2e-4 2e-5
%     1.5e-5 0 2e-3
%     -1.5e-4 1.5e-3 0];
% par_trajGen.IMU.acc.SO = [5e-5 -6e-6 7e-7]' / par_trajGen.g; % 转换前单位1/g，转换后单位1/(m/s^2)

% 加速度计随机误差
par_trajGen.IMU.acc.noise.R = 0 * ones(3, 1) * (par_trajGen.g/1e6) / 3600; % 转换前单位μg/h，转换后单位m/s^3
par_trajGen.IMU.acc.noise.K = 0 * ones(3, 1) * (par_trajGen.g/1e6) / 60; % 转换前单位μg/sqrt(h)，转换后单位m/s^(5/2)
par_trajGen.IMU.acc.noise.N = 0 * ones(3, 1) * (par_trajGen.g/1e6) * 60; % 转换前单位μg*sqrt(h)，转换后单位m/s^(3/2)
par_trajGen.IMU.acc.noise.Q = realmin * ones(3, 1) / 3600; % 转换前单位m/h，转换后单位m/s
par_trajGen.IMU.acc.noise.qc = 0 * ones(3, 1) * (par_trajGen.g/1e6) / 60; % 转换前单位μg/sqrt(h)，转换后单位m/s^(5/2)
par_trajGen.IMU.acc.noise.Tc = Inf * ones(3, 1) * 3600; % 转换前单位h，转换后单位s
par_trajGen.IMU.acc.noise.Omega0 = 0 * ones(3, 1) * (par_trajGen.g/1e6); % 转换前单位μg，转换后单位m/s^2
par_trajGen.IMU.acc.noise.f0 = ones(3, 1); % 单位Hz

% 陀螺常值误差
par_trajGen.IMU.gyro.PS0B = eye(3);
par_trajGen.IMU.gyro.KScal0 = ones(3, 1);
par_trajGen.IMU.gyro.domegaIBBiasS = zeros(3, 1);
par_trajGen.IMU.gyro.dKScalP = [zeros(3, 1) zeros(3, 1)];
par_trajGen.IMU.gyro.dKScalN = [zeros(3, 1) zeros(3, 1)];
par_trajGen.IMU.gyro.dPSS0 = zeros(3);
par_trajGen.IMU.gyro.domegaIBQuantS = zeros(3, 1);
par_trajGen.IMU.gyro.DGSens = zeros(3);
% par_trajGen.IMU.gyro.B = [-5 2 -0.3]' * cst.sec2Rad; % 转换前单位°/h，转换后单位rad/s
% par_trajGen.IMU.gyro.SF0 = [1 1 1]' / cst.sec2Rad; % 转换前单位(^/s)/(°/h)，转换后单位(^/s)/(rad/s)
% par_trajGen.IMU.gyro.dSFp = [-2e-6 2e-5 -2e-4]';
% par_trajGen.IMU.gyro.dSFn = [-2e-6 2e-5 -2e-4]';
% par_trajGen.IMU.gyro.MA = [0 7e-3 -6e-4
%     5e-5 0 -4e-5
%     3e-4 -2e-3 0];
% par_trajGen.IMU.gyro.GS = [7e-1 -6e-2 0
%     6e-2 7e-1 0
%     0 3e-2 -2e-1] * cst.sec2Rad / par_trajGen.g; % 转换前单位°/h/g，转换后单位rad/s/(m/s^2)

% 陀螺随机误差
par_trajGen.IMU.gyro.noise.R = 0 * ones(3, 1) * cst.deg2Rad / (3600^2); % 转换前单位°/h^2，转换后单位rad/s^2
par_trajGen.IMU.gyro.noise.K = 0 * ones(3, 1) * cst.deg2Rad / (3600^(3/2)); % 转换前单位°/h^(3/2)，转换后单位rad/s^(3/2)
par_trajGen.IMU.gyro.noise.N = 0 * ones(3, 1) * cst.deg2Rad / 60; % 转换前单位°/sqrt(h)，转换后单位rad/sqrt(s)
par_trajGen.IMU.gyro.noise.Q = realmin * ones(3, 1) * cst.sec2Rad; % 转换前单位″，转换后单位rad
par_trajGen.IMU.gyro.noise.qc = 0 * ones(3, 1) * cst.deg2Rad / (3600^(3/2)); % 转换前单位°/h^(3/2)，转换后单位rad/s^(3/2)
par_trajGen.IMU.gyro.noise.Tc = Inf * ones(3, 1) * 3600; % 转换前单位h，转换后单位s
par_trajGen.IMU.gyro.noise.Omega0 = 0 * ones(3, 1) * cst.sec2Rad; % 转换前单位°/h，转换后单位rad/s
par_trajGen.IMU.gyro.noise.f0 = ones(3, 1); % 单位Hz
% 惯组杆臂
par_trajGen.IMU.lV = 1*[ 200.0; 100.0; 300.0 ];
% 位移计杆臂
par_trajGen.displm.lV = 1*[50; 100; 200];

%% 初始条件
% 测试
iniCon_trajGen.CVG = angle2dcm(0*cst.deg2Rad, 0*cst.deg2Rad, 0*cst.deg2Rad, 'ZXY')'; % 初始姿态矩阵
iniCon_trajGen.CBV = eye(3);
iniCon_trajGen.vG = zeros(3, 1); % 初始速度
iniCon_trajGen.pos = [30 * cst.deg2Rad, 110 * cst.deg2Rad, 100]'; % 纬度、经度、高度

%% 运行仿真
[out_trajGen, auxOut_trajGen] = gentrajectory(in_trajGen, cfg_trajGen, par_trajGen, iniCon_trajGen, cst_trajGen);

%% 后处理
close all
if cfg_trajGen.enableNavigation
    comparepos({auxOut_trajGen.nav.pos}, {out_trajGen.t}, out_trajGen.pos(:, :, 1), out_trajGen.t, 'cmpName', {'比较值：导航值'}, 'refName', '参考值：轨迹原点');
    comparepos({auxOut_trajGen.nav.pos}, {out_trajGen.t}, out_trajGen.pos(:, :, 2), out_trajGen.t, 'cmpName', {'比较值：导航值'}, 'refName', '参考值：位移计杆臂点');
    comparepos({auxOut_trajGen.nav.pos}, {out_trajGen.t}, out_trajGen.pos(:, :, 3), out_trajGen.t, 'cmpName', {'比较值：导航值'}, 'refName', '参考值：惯组杆臂点');
    %
    comparevector({auxOut_trajGen.nav.vG}, {out_trajGen.t}, out_trajGen.vG(:, :, 1), out_trajGen.t, 'cmpName', {'比较值：导航值'}, 'refName', '轨迹原点');
    comparevector({auxOut_trajGen.nav.vG}, {out_trajGen.t}, out_trajGen.vG(:, :, 2), out_trajGen.t, 'cmpName', {'比较值：导航值'}, 'refName', '位移计杆臂点');
    comparevector({auxOut_trajGen.nav.vG}, {out_trajGen.t}, out_trajGen.vG(:, :, 3), out_trajGen.t, 'cmpName', {'比较值：导航值'}, 'refName', '惯组杆臂点');
    %
    compareatt_dcm({auxOut_trajGen.nav.CBG}, {out_trajGen.t}, out_trajGen.CBG, out_trajGen.t, [], {'比较值：导航值'}, '轨迹值');
end
%
comparepos({out_trajGen.pos(:, :, 3)}, {out_trajGen.t}, out_trajGen.pos(:, :, 1), out_trajGen.t, 'cmpName', {'比较值：惯组杆臂点'}, 'refName', '参考值：轨迹原点');
comparepos({out_trajGen.pos(:, :, 2)}, {out_trajGen.t}, out_trajGen.pos(:, :, 1), out_trajGen.t, 'cmpName', {'比较值：位移计臂点'}, 'refName', '参考值：轨迹原点');
preparefig('位置比较');
subplot(2, 1, 1);plot(out_trajGen.t, out_trajGen.RE(:,:,2)-out_trajGen.RE(:,:,1));xlabel('时间(s)');ylabel('(m)');title('位移计杆臂点相对于轨迹原点地球系下位置比较');
subplot(2, 1, 2);plot(out_trajGen.t, out_trajGen.RE(:,:,3)-out_trajGen.RE(:,:,1));xlabel('时间(s)');ylabel('(m)');title('惯组杆臂点相对于轨迹原点地球系下位置比较');
comparevector({out_trajGen.vG(:, :, 3)}, {out_trajGen.t}, out_trajGen.vG(:, :, 1), out_trajGen.t, 'cmpName', {'比较值：惯组杆臂点'}, 'refName', '轨迹原点');
comparevector({out_trajGen.vG(:, :, 2)}, {out_trajGen.t}, out_trajGen.vG(:, :, 1), out_trajGen.t, 'cmpName', {'比较值：位移计臂点'}, 'refName', '轨迹原点');

% 姿态
preparefig('姿态曲线');
subplot(2, 2, 1);
plot(out_trajGen.t, out_trajGen.vehicleAtt/cst.deg2Rad);title('V系');xlabel('时间(s)');ylabel('(°)');
legend('航向' ,'俯仰' ,'滚转');
subplot(2, 2, 2);
plot(out_trajGen.t,mod(out_trajGen.bodyAtt/cst.deg2Rad,360));title('B系');xlabel('时间(s)');ylabel('(°)');
subplot(2, 2, 3);
plot(out_trajGen.t,mod((out_trajGen.bodyAtt - out_trajGen.vehicleAtt)/cst.deg2Rad,360));title('B系相对于V系偏差');xlabel('时间(s)');ylabel('(°)');
% 速度
preparefig('速度曲线');
subplot(2, 2, 1);
plot(out_trajGen.t,out_trajGen.vG(:, :, 1));title('轨迹原点');xlabel('时间(s)');ylabel('(m/s)');
legend('东向' ,'北向' ,'天向');
subplot(2, 2, 2);
plot(out_trajGen.t,out_trajGen.vG(:, :, 3));title('惯组杆臂点');xlabel('时间(s)');ylabel('(m/s)');
legend('东向' ,'北向' ,'天向');
subplot(2, 2, 3);
plot(out_trajGen.t,out_trajGen.vG(:, :, 2));title('位移计杆臂点');xlabel('时间(s)');ylabel('(m/s)');
legend('东向' ,'北向' ,'天向');
subplot(2, 2, 4);
plot(out_trajGen.t,out_trajGen.vG(:, :, 3) - out_trajGen.vG(:, :, 1) );title('惯组杆臂点相对于轨迹原点偏差');xlabel('时间(s)');ylabel('(m/s)');

% 加速度
preparefig('加速度曲线');
subplot(2, 2, 1);
plot(out_trajGen.t,out_trajGen.fV(:, :, 1));title('轨迹原点');xlabel('时间(s)');ylabel('(m/s/s)');
subplot(2, 2, 2);
plot(out_trajGen.t,out_trajGen.fV(:, :, 3));title('惯组杆臂点');xlabel('时间(s)');ylabel('(m/s/s)');
subplot(2, 2, 3);
plot(out_trajGen.t,out_trajGen.fV(:, :, 3) - out_trajGen.fV(:, :, 1));title('惯组杆臂点相对于轨迹原点偏差');xlabel('时间(s)');ylabel('(m/s/s)');
subplot(2, 2, 4);
plot(out_trajGen.t,out_trajGen.fV(:, :, 3) - out_trajGen.IMU.fBErrFree);title('惯组杆臂点相对于惯组输出偏差');xlabel('时间(s)');ylabel('(m/s/s)');

% 角速率(omegaIBB-omegaIVV)
preparefig('角速率曲线');
subplot(2, 2, 1);
plot(out_trajGen.t,out_trajGen.omegaIVV/cst.deg2Rad);title('\omegaIVV');xlabel('时间(s)');ylabel('(°/s)');
subplot(2, 2, 2);
plot(out_trajGen.t,out_trajGen.IMU.omegaIBBErrFree/cst.deg2Rad);title('\omegaIBB');xlabel('时间(s)');ylabel('(°/s)');
subplot(2, 2, 3);
plot(out_trajGen.t,(out_trajGen.omegaIVV - out_trajGen.IMU.omegaIBBErrFree)/cst.deg2Rad);title('\omegaIVV - \omegaIBB');xlabel('时间(s)');ylabel('(°/s)');

% 里程
preparefig('里程曲线');
subplot(2, 3, 1);
plot(out_trajGen.t,out_trajGen.s(:, 1));title('轨迹原点');xlabel('时间(s)');ylabel('(m)');grid on;
subplot(2, 3, 2);
plot(out_trajGen.t,out_trajGen.s(:, 3));title('惯组杆臂点');xlabel('时间(s)');ylabel('(m)');grid on;
subplot(2, 3, 3);
plot(out_trajGen.t,out_trajGen.s(:, 2));title('位移计杆臂点');xlabel('时间(s)');ylabel('(m)');grid on;
subplot(2, 3, 4);
plot(out_trajGen.t,out_trajGen.s(:, 2) - out_trajGen.s(:, 1) );title('位移计杆臂点相对于轨迹原点偏差');xlabel('时间(s)');ylabel('(m)');grid on;
subplot(2, 3, 5);
plot(out_trajGen.t,out_trajGen.s(:, 3) - out_trajGen.s(:, 1) );title('惯组杆臂点相对于轨迹原点偏差');xlabel('时间(s)');ylabel('(m)');grid on;

%% 保存曲线
if cfg_trajGen.enableCBVdynamic
    FilePath_CBV = 'CBVdynamic(1)';
else
    FilePath_CBV = 'CBVdynamic(0)';
end
%
if any(par_trajGen.IMU.lV)
    FilePath_IMUlV = 'IMUlV(1)';
else
    FilePath_IMUlV = 'IMUlV(0)';
end
%
if any(par_trajGen.displm.lV)
    FilePath_displmlV = 'displmlV(1)';
else
    FilePath_displmlV = 'displmlV(0)';
end

DataFilePath = [ pwd '\' in_trajGen.genFuncName '\' FilePath_CBV FilePath_IMUlV FilePath_displmlV datestr(now, 'yyyy-mm-ddTHH-MM-SS') ];
if ~isfolder(DataFilePath)
    mkdir(DataFilePath);
end
saveallfigs([DataFilePath '\'], 'fig', false);

%% 保存轨迹原始数据
save([DataFilePath '\trajGen'], 'out_trajGen', 'auxOut_trajGen');
